using System;
using System.Collections.Generic;
using System.Text;
using Vj.OdeObjectLibrary;
using Vj.MyMath;
using Tao.Ode;
using System.Drawing;
using System.Windows.Forms;
using Vj.GA;

namespace WalkingApp
{

    public class AgentWalkInfo
    {
        public PointF[] leg1Angle1 = null;
        public PointF[] leg1Angle2 = null;
        public PointF[] leg1Angle3 = null;

        public PointF[] leg2Angle1 = null;
        public PointF[] leg2Angle2 = null;
        public PointF[] leg2Angle3 = null;

        public PointF[] leg3Angle1;
        public PointF[] leg3Angle2;
        public PointF[] leg3Angle3;

        public PointF[] leg4Angle1;
        public PointF[] leg4Angle2;
        public PointF[] leg4Angle3;

        public PointF[] leg5Angle1;
        public PointF[] leg5Angle2;
        public PointF[] leg5Angle3;

        public PointF[] leg6Angle1;
        public PointF[] leg6Angle2;
        public PointF[] leg6Angle3;
    }


    public class FourierWalk
    {

        // define for each joint a fourier series
        // define a chromosome that encapsulates above values

        // randomize population of chromosomes
        // using the constants (chrom) for the fourier series... 
        //   calculate the list of force (or position) values to simulate
        // run simulation for each chromosome, rate by...
        //   distance walked, speed, accel?
        // choose some proportion of best individuals
        // breed + mutate
        // create new population


        Hexapod[] agentList;
        private int counter = 0;
        AgentWalkInfo[] agentsWalkList;

        public FourierWalk(IntPtr world, IntPtr space)
        {
            int numOfAgents = 3;
            agentList = new Hexapod[numOfAgents];

            for (int i = 0; i < agentList.Length; i++)
            {
                agentList[i] = new Hexapod(world, space, 1.0f, 10.0f,
                    new Ode.dVector3(-10.0f + 10.0f * (float)i, 2.0f, 0.0f),
                    new Ode.dVector3(153.0f / 255.0f, 51.0f / 255.0f, 0.0f));
            }

            DefineWalks();
        }

        public void Draw()
        {
                for (int i = 0; i < agentList.Length; i++)
                    agentList[i].Render();
        }


        public void IncrementAgents()
        {
            for (int i = 0; i < agentList.Length; i++)
            {
                agentList[i].SetJointTorque(LegPositions.Front, Side.Left, JointPosition.Upper,
                    new float[] { agentsWalkList[i].leg1Angle1[counter].Y, agentsWalkList[i].leg1Angle2[counter].Y });

                agentList[i].SetJointTorque(LegPositions.Front, Side.Left, JointPosition.Lower,
                    new float[] { agentsWalkList[i].leg1Angle3[counter].Y });


                agentList[i].SetJointTorque(LegPositions.Mid, Side.Left, JointPosition.Upper,
                    new float[] { agentsWalkList[i].leg2Angle1[counter].Y, agentsWalkList[i].leg2Angle2[counter].Y });

                agentList[i].SetJointTorque(LegPositions.Mid, Side.Left, JointPosition.Lower,
                    new float[] { agentsWalkList[i].leg2Angle3[counter].Y });


                agentList[i].SetJointTorque(LegPositions.Back, Side.Left, JointPosition.Upper,
                    new float[] { agentsWalkList[i].leg3Angle1[counter].Y, agentsWalkList[i].leg3Angle2[counter].Y });

                agentList[i].SetJointTorque(LegPositions.Back, Side.Left, JointPosition.Lower,
                    new float[] { agentsWalkList[i].leg3Angle3[counter].Y });

                // -----------------

                agentList[i].SetJointTorque(LegPositions.Front, Side.Right, JointPosition.Upper,
                    new float[] { agentsWalkList[i].leg4Angle1[counter].Y, agentsWalkList[i].leg4Angle2[counter].Y });

                agentList[i].SetJointTorque(LegPositions.Front, Side.Right, JointPosition.Lower,
                    new float[] { agentsWalkList[i].leg4Angle3[counter].Y });


                agentList[i].SetJointTorque(LegPositions.Mid, Side.Right, JointPosition.Upper,
                    new float[] { agentsWalkList[i].leg5Angle1[counter].Y, agentsWalkList[i].leg5Angle2[counter].Y });

                agentList[i].SetJointTorque(LegPositions.Mid, Side.Right, JointPosition.Lower,
                    new float[] { agentsWalkList[i].leg5Angle3[counter].Y });


                agentList[i].SetJointTorque(LegPositions.Back, Side.Right, JointPosition.Upper,
                    new float[] { agentsWalkList[i].leg6Angle1[counter].Y, agentsWalkList[i].leg6Angle2[counter].Y });

                agentList[i].SetJointTorque(LegPositions.Back, Side.Right, JointPosition.Lower,
                    new float[] { agentsWalkList[i].leg6Angle3[counter].Y });

            }

            counter++;

            if (counter >= agentsWalkList[0].leg1Angle1.Length)
                counter = 0;
        }

        public void DefineWalks()
        {
            float[] paramList = new float[11];

            float minBounds = -12.0f;
            float maxBounds = 12.0f;

            for (int i = 0; i < paramList.Length; i++)
                paramList[i] = (float)RandomGrabber.Next(minBounds, maxBounds);
            
            agentsWalkList = new AgentWalkInfo[3];
            for (int i = 0; i < 3; i++)
                agentsWalkList[i] = new AgentWalkInfo();


            for (int i = 0; i < agentsWalkList.Length; i++)
            {
                
                agentsWalkList[i].leg1Angle1 = FourierSeries(20, paramList);
                
                for (int j = 0; j < paramList.Length; j++)
                    paramList[j] = (float)RandomGrabber.Next(minBounds, maxBounds);

                agentsWalkList[i].leg1Angle2 = FourierSeries(20, paramList);

                for (int j = 0; j < paramList.Length; j++)
                    paramList[j] = (float)RandomGrabber.Next(minBounds, maxBounds);

                agentsWalkList[i].leg1Angle3 = FourierSeries(20, paramList);

                for (int j = 0; j < paramList.Length; j++)
                    paramList[j] = (float)RandomGrabber.Next(minBounds, maxBounds);


                agentsWalkList[i].leg2Angle1 = FourierSeries(20, paramList);

                for (int j = 0; j < paramList.Length; j++)
                    paramList[j] = (float)RandomGrabber.Next(minBounds, maxBounds);

                agentsWalkList[i].leg2Angle2 = FourierSeries(20, paramList);

                for (int j = 0; j < paramList.Length; j++)
                    paramList[j] = (float)RandomGrabber.Next(minBounds, maxBounds);

                agentsWalkList[i].leg2Angle3 = FourierSeries(20, paramList);

                for (int j = 0; j < paramList.Length; j++)
                    paramList[j] = (float)RandomGrabber.Next(minBounds, maxBounds);

                
                
                agentsWalkList[i].leg3Angle1 = FourierSeries(20, paramList);

                for (int j = 0; j < paramList.Length; j++)
                    paramList[j] = (float)RandomGrabber.Next(minBounds, maxBounds);

                agentsWalkList[i].leg3Angle2 = FourierSeries(20, paramList);

                for (int j = 0; j < paramList.Length; j++)
                    paramList[j] = (float)RandomGrabber.Next(minBounds, maxBounds);

                agentsWalkList[i].leg3Angle3 = FourierSeries(20, paramList);

                for (int j = 0; j < paramList.Length; j++)
                    paramList[j] = (float)RandomGrabber.Next(minBounds, maxBounds);



                agentsWalkList[i].leg4Angle1 = FourierSeries(20, paramList);

                for (int j = 0; j < paramList.Length; j++)
                    paramList[j] = (float)RandomGrabber.Next(minBounds, maxBounds);

                agentsWalkList[i].leg4Angle2 = FourierSeries(20, paramList);

                for (int j = 0; j < paramList.Length; j++)
                    paramList[j] = (float)RandomGrabber.Next(minBounds, maxBounds);

                agentsWalkList[i].leg4Angle3 = FourierSeries(20, paramList);

                for (int j = 0; j < paramList.Length; j++)
                    paramList[j] = (float)RandomGrabber.Next(minBounds, maxBounds);
                
                
                
                agentsWalkList[i].leg5Angle1 = FourierSeries(20, paramList);

                for (int j = 0; j < paramList.Length; j++)
                    paramList[j] = (float)RandomGrabber.Next(minBounds, maxBounds);

                agentsWalkList[i].leg5Angle2 = FourierSeries(20, paramList);

                for (int j = 0; j < paramList.Length; j++)
                    paramList[j] = (float)RandomGrabber.Next(minBounds, maxBounds);
                
                agentsWalkList[i].leg5Angle3 = FourierSeries(20, paramList);

                for (int j = 0; j < paramList.Length; j++)
                    paramList[j] = (float)RandomGrabber.Next(minBounds, maxBounds);



                agentsWalkList[i].leg6Angle1 = FourierSeries(20, paramList);

                for (int j = 0; j < paramList.Length; j++)
                    paramList[j] = (float)RandomGrabber.Next(minBounds, maxBounds);
                
                agentsWalkList[i].leg6Angle2 = FourierSeries(20, paramList);

                for (int j = 0; j < paramList.Length; j++)
                    paramList[j] = (float)RandomGrabber.Next(minBounds, maxBounds);

                agentsWalkList[i].leg6Angle3 = FourierSeries(20, paramList);

            }
               
        }


        private PointF[] FourierSeries(int length, float[] paramList)
        {
            if (paramList.Length != 11)
                throw new Exception("Invalid paramList");

            float a0 = paramList[0];
            float a1 = paramList[1]; float b1 = paramList[2];
            float a2 = paramList[3]; float b2 = paramList[4];
            float a3 = paramList[5]; float b3 = paramList[6];
            float a4 = paramList[7]; float b4 = paramList[8];
            float a5 = paramList[9]; float b5 = paramList[10];

            PointF[] f = new PointF[length];

            float L = (float)Math.PI;

            for (int x = 0; x < f.Length; x++)
            {
                float i = 2 * (float)Math.PI * x / (float)f.Length;

                f[x].X = i;

                f[x].Y = a0 / 2.0f + (float)(a1 * Math.Cos(1.0f * Math.PI * (float)i / L) + b1 * Math.Sin(1.0f * Math.PI * (float)i / L)) +
                                     (float)(a2 * Math.Cos(2.0f * Math.PI * (float)i / L) + b2 * Math.Sin(2.0f * Math.PI * (float)i / L)) +
                                     (float)(a3 * Math.Cos(3.0f * Math.PI * (float)i / L) + b3 * Math.Sin(3.0f * Math.PI * (float)i / L)) +
                                     (float)(a4 * Math.Cos(4.0f * Math.PI * (float)i / L) + b4 * Math.Sin(4.0f * Math.PI * (float)i / L)) +
                                     (float)(a5 * Math.Cos(5.0f * Math.PI * (float)i / L) + b5 * Math.Sin(5.0f * Math.PI * (float)i / L));
            }

            return f;
        }

        public void GraphFourier()
        {
            float [] paramList = new float[11];
            for (int i = 0; i < paramList.Length; i++)
                paramList[i] = (float)RandomGrabber.Next(-5.0, 5.0);

            PointF[] results = FourierSeries(100, paramList);
            Bitmap bm = new Bitmap(400, 400);

            int w = (int)bm.Width;
            int h = (int)bm.Height;

            Graphics g = Graphics.FromImage(bm);
            
            for (int x = 0; x < results.Length-1; x++)
            {
                Point pt1 = new Point((int)(20.0f * results[x].X + w / 2.0f),
                                      (int)(h / 2.0f - 20.0f * results[x].Y));

                Point pt2 = new Point((int)(20.0f * results[x+1].X + w / 2.0f),
                                      (int)(h / 2.0f - 20.0f * results[x+1].Y));

                g.DrawLine(Pens.Black, pt1, pt2);
            }

            bm.Save(@"C:\Temp\output.bmp");
        }


    }
}
